//
// Created by yu on 2023/12/8.
//

#include <iostream>

#include "mi-motor.h"
#include "RobotMath.h"

typedef double d;
using namespace Eigen;

miMotor::miReturnData RobotMath::LowPassFilter(float alpha, miMotor::miReturnData data)
{

    data.torque = data.torque * (1 - alpha) + formerData.torque * alpha;
    data.angularVelocity = data.angularVelocity * (1 - alpha) + formerData.angularVelocity * alpha;

    formerData = data;

    return data;
}

Eigen::Vector4d RobotMath::LQRCalculator(double legLength)
{
    double g = 9.8;
    double m = 2.65;
    double M = 0.5;
    double l = legLength;
    double b = 0.8;
    double J = m * l * l;
    double p = J * (M + m) + M * m * l * l;

    double A22 = -(J + m * l * l) * b / p;
    double A23 = (m * m * g * l * l) / p;
    double A42 = -(m * l * b) / p;
    double A43 = m * g * l * (M + m) / p;
    Matrix4d A;
    A << 0, 1, 0, 0, 0, A22, A23, 0, 0, 0, 0, 1, 0, A42, A43, 0;

    double B2 = (J + m * l * l) / p;
    double B4 = m * l / p;
    RowVector4d B;
    B << 0, B2, 0, B4;

    Matrix4d Q; // 对状态变量进行加权的权重矩阵
    Q << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 100, 0, 0, 0, 0, 1;
    double R = 1;
    MatrixXd I = MatrixXd::Identity(A.rows(), A.cols());

    // 初始化性能指标矩阵S为单位矩阵
    Matrix4d S = Q;
    MatrixXd K;
    // 迭代求解Riccati方程
    int max_iter = 100;
    double epsilon = 1e-3;
    for (int i = 0; i < max_iter; ++i)
    {
        // 计算增益矩阵K
        K = (R * I + B * S * B.transpose()).inverse() * B * S * A;

        // 更新Riccati方程的解S
        S = Q + A.transpose() * S * (A - B.transpose() * K);

        // 判断迭代收敛
        if ((S - S.transpose()).norm() < epsilon)
        {
            break;
        }
    }
    return K;
}
